//
// Created by ze on 2023/11/2.
//

#include "UPRE_LOCATION.h"
#include "UPRE_GYRO100A.h"
#include "tim.h"


//全场位姿
Pose Robot_Pose = {0};
//全场定位结构体
Location location = {0};
void Encoder_On()
{
    //码盘读数
    static int32_t Enc1,Enc2,Last_Enc1,Last_Enc2;
    Enc1 = __HAL_TIM_GET_COUNTER(&htim1);
    Enc2 = __HAL_TIM_GET_COUNTER(&htim3);
    //码盘脉冲数微分值

        if(fabsf((float)Enc1 - (float)Last_Enc1) >= 50000.f)
            location.DIS_1 = 0;
        else
            location.DIS_1 =   (float) (Enc1 - Last_Enc1);  //X轴
        if(fabsf((float)Enc2 - (float)Last_Enc2) >= 50000.f)
            location.DIS_2 = 0;
        else
            location.DIS_2 =   (float)-(Enc2 - Last_Enc2);  //Y轴

#if Printf_Pose == 1
    printf("DIS1:%f\t DIS2:%f\t Whole_X:%f\t Whole_y:%f\t buf_x:%f\t buf_y:%f\tYaw:%f\n",location.DIS_1,location.DIS_2,location.Whole_X,location.Whole_Y,location.buf_x,location.buf_y,Robot_Pose.yaw);

#endif
    /******************微分***********************/
    Last_Enc1 = Enc1;
    Last_Enc2 = Enc2;
}
void Location_Init(void)
{
    /*************************不要动************************/
    location.theta = Angle2Radian(45.f);
    /*****************************************************/
    location.gyro_radian = Angle2Radian(gyro.Angle_azimuth);
    location.theta1 = Angle2Radian(Tt1)  + location.gyro_radian;
    location.theta2 = Angle2Radian(Tt2)  + location.gyro_radian;
    location.Sin_12 = sinf(Angle2Radian(Tt1 - Tt2));
    location.Sin_21 = sinf(Angle2Radian(Tt2 - Tt1));
    //距离标定法
//    location.R1 = 100.5f;
//    location.R2 = 100.5f;
//    location.fail1 = Angle2Radian(150.f);
//    location.fail2 = Angle2Radian(36.f);
    //旋转中心标定法
//    location.Center_X = 0.f;
//    location.Center_Y = 0.f;
    location.Center_X = -105.f;
    location.Center_Y = 2.f;
//    location.RECTANG_X = -0.0132264529f-0.0028152021f;
//    location.RECTANG_X = 0.f;
//    location.RECTANG_Y = 0.f;
//    location.k1 =   arm_cos_f32(location.theta) * PULSE2DIS_ENCODE1  - arm_sin_f32(location.theta) * PULSE2DIS_ENCODE2  * location.RECTANG_X;
//    location.k2 =   arm_sin_f32(location.theta) * PULSE2DIS_ENCODE1  + arm_cos_f32(location.theta) * PULSE2DIS_ENCODE2  * location.RECTANG_X;
//    location.k3 = - arm_sin_f32(location.theta) * PULSE2DIS_ENCODE2  + arm_cos_f32(location.theta) * PULSE2DIS_ENCODE1  * location.RECTANG_Y;
//    location.k4 =   arm_cos_f32(location.theta) * PULSE2DIS_ENCODE2  + arm_sin_f32(location.theta) * PULSE2DIS_ENCODE1  * location.RECTANG_Y;
//    location.g_theta1 = atan2f(-location.k2,location.k4);
//    location.g_theta2 = atan2f(-location.k1,location.k3);

//location.g_pulse_dis1 = location.k1 / (PULSE2DIS_ENCODE1 * arm_sin_f32(location.g_theta2) / location.Sin_21);
//location.g_pulse_dis2 = location.k2 / (PULSE2DIS_ENCODE2 * (-arm_sin_f32(location.g_theta1)) / location.Sin_21);

}
//全场标定函数
void cal_location(void)
{
    //逆时针旋转

    Robot_Pose.yaw = gyro.Angle_azimuth;
    location.gyro_radian = Angle2Radian(Robot_Pose.yaw);
    location.ALPHA = location.gyro_radian -location.alphaed;
    location.theta1 = Angle2Radian(Tt1)  + location.gyro_radian;
    location.theta2 = Angle2Radian(Tt2)  + location.gyro_radian;
    /**************************旋转矩阵**********************************************************/
//逆时针旋转矩阵
    location.buf_x = ( (float)location.DIS_1 * cosf(location.theta) + (float)location.DIS_2 * sinf(location.theta)) * PULSE2DIS_ENCODE1;
    location.buf_y = (-(float)location.DIS_1 * sinf(location.theta) + (float)location.DIS_2 * cosf(location.theta)) * PULSE2DIS_ENCODE2;
//    Robot_Pose.wx += (float)location.buf_x;
//    Robot_Pose.wy += (float)location.buf_y;
//    location.spi_Posx = location.buf_x + location.buf_y * location.RECTANG_X;
//    location.spi_Posy = location.buf_y + location.buf_x * location.RECTANG_Y;

    //相对标定法：相对 and 全场直线
//    location.spi_Posx = location.k1 * location.DIS_1 + location.k2 * location.DIS_2;
//    location.spi_Posy = location.k3 * location.DIS_1 + location.k4 * location.DIS_2;

//全场
//location.spi_Posx = (-(float)location.buf_x * arm_sin_f32(location.theta2) + (float)location.buf_y * arm_sin_f32(location.theta1)) / location.Sin_12;
//location.spi_Posy = (-(float)location.buf_x * arm_cos_f32(location.theta2) + (float)location.buf_y * arm_cos_f32(location.theta1)) / location.Sin_21;

    //全场反解公式
//location.spi_Posx = ( DIS2 * sint1 - DIS1 * sint2 + _L1 * arm_cos_f32(location.fail1) *
//        sint2 - _L2 * arm_cos_f32(location.fail2) * sint1 ) / location.Sin_12;
//location.spi_Posy = ( DIS2 * cost1 - DIS1 * cost2 + _L1 *
//        arm_cos_f32(location.fail1) * cost2 - _L2 * arm_cos_f32(location.fail2) *
//        cost1 ) / location.Sin_21;
//全场微分
    location.m_spi_Posx = ( DIS2  * sint1 - DIS1   * sint2  ) / location.Sin_12;
    location.m_spi_Posy = ( DIS2  * cost1 - DIS1   * cost2  ) / location.Sin_21;
//积分部分
    location.spi_Posx += location.m_spi_Posx ;
    location.spi_Posy += location.m_spi_Posy ;
//坐标平移变换
    location.Whole_X = location.spi_Posx - location.Center_X + location.Center_X *
            cosf(location.gyro_radian) - location.Center_Y *
            sinf(location.gyro_radian);
    location.Whole_Y = location.spi_Posy - location.Center_Y + location.Center_Y *
            cosf(location.gyro_radian) + location.Center_X *
            sinf(location.gyro_radian);
    Robot_Pose.wx = (float)location.Whole_X;
    Robot_Pose.wy = (float)location.Whole_Y;
    /******************微分***********************/
location.alphaed = location.gyro_radian;
}